#include "OV.hpp"

OV_V4L2::OV_V4L2() : dev_fd_(-1), lcd_fd_(-1), lcdptr_(nullptr), lcd_w_(-1), lcd_h_(-1) 
{
    // 打开设备
    dev_fd_ = open("/dev/video0", O_RDWR);
    if (dev_fd_ < 0)
    {
        perror("open video0 err");
        exit(1);
    }

    lcd_fd_ = open("/dev/fb0", O_RDWR);
    if(lcd_fd_ < 0)
    {
            perror("open fb0");
            exit(1);
    }
        
    //获取lcd信息
    struct fb_var_screeninfo info;
    int lret = ioctl(lcd_fd_, FBIOGET_VSCREENINFO, &info);
        
    //开发板-arm
    lcd_w_ = info.xres;
    lcd_h_ = info.yres;
        
    lcdptr_ = (unsigned int*)mmap(NULL, lcd_w_*lcd_h_*4, PROT_READ|PROT_WRITE, MAP_SHARED, lcd_fd_, 0);

    // 初始化mem结构体
    v_mem_.resize(MEM_COUNT);
    for (int i = 0; i < 4; i++)
    {
        v_mem_[i].mptr = nullptr;
        v_mem_[i].size = 0;
    }
}

// 获取摄像头支持的格式
bool OV_V4L2::get_fmt_desc()
{
    struct v4l2_fmtdesc v4fmt_desc;
    memset(&v4fmt_desc, 0, sizeof(v4fmt_desc));

    int i = 0;
    while (1)
    {
        v4fmt_desc.index = i++;
        v4fmt_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        if (ioctl(dev_fd_, VIDIOC_ENUM_FMT, &v4fmt_desc) < 0)
            return false;
        cout << "index: " << v4fmt_desc.index << endl;
        cout << "type: " << v4fmt_desc.type << endl;
        cout << "flags: " << v4fmt_desc.flags << endl;
        cout << "pixelformat: " << v4fmt_desc.pixelformat << endl;
        cout << "description: " << v4fmt_desc.description << endl;
        cout << "reserved: " << v4fmt_desc.reserved[0] << endl;
    }

    return true;
}

// 获取摄像头设置的格式
void OV_V4L2::get_fmt(struct v4l2_format *v4l2_fmt)
{
    if (v4l2_fmt == nullptr)
        return;
    memset(v4l2_fmt, 0, sizeof(*v4l2_fmt));
    v4l2_fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    int ret = ioctl(dev_fd_, VIDIOC_G_FMT, v4l2_fmt);
    if (ret < 0)
    {
        perror("get format error");
    }
}

// 设置摄像头格式
bool OV_V4L2::set_fmt()
{
    struct v4l2_format v4l2_fmt;
    memset(&v4l2_fmt, 0, sizeof(struct v4l2_format));
    v4l2_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    v4l2_fmt.fmt.pix_mp.height = lcd_h_;
    v4l2_fmt.fmt.pix_mp.width = lcd_w_;
    v4l2_fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_UYVY;
    v4l2_fmt.fmt.pix_mp.field = V4L2_FIELD_ANY;

    int ret = ioctl(dev_fd_, VIDIOC_S_FMT, &v4l2_fmt);
    if (ret < 0)
    {
        perror("set format error");
        return false;
    }

    // 获取当前格式
    get_fmt(&v4l2_fmt);

    // 配置完成后，通过VIDIOC_G_FMT命令读取当前的配置进行验证
    if (v4l2_fmt.fmt.pix.width == lcd_w_ && v4l2_fmt.fmt.pix.height == lcd_h_ &&
        v4l2_fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY)
    {
        cout << "camera set success\n"
             << endl;
    }
    else
    {
        cout << "camera set error\n"
             << endl;
    }
    return true;
}

// 申请内核空间映射到用户空间
bool OV_V4L2::rqMemorytoUsr()
{
    // 申请内核空间
    struct v4l2_requestbuffers rq_buff;
    rq_buff.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    rq_buff.count = MEM_COUNT;
    rq_buff.memory = V4L2_MEMORY_MMAP;

    int ret = ioctl(dev_fd_, VIDIOC_REQBUFS, &rq_buff);
    if (ret < 0)
    {
        perror("requst memory error");
        return false;
    }

    // 映射到用户空间
    struct v4l2_buffer map_buff;
    struct v4l2_plane planes[1];
    map_buff.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    for (int i = 0; i < 4; i++)
    {
        memset(&map_buff, 0, sizeof(map_buff));
        memset(planes, 0, sizeof(planes));

        map_buff.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        map_buff.memory = V4L2_MEMORY_MMAP;
        map_buff.index = i;
        map_buff.m.planes = planes;
        map_buff.length = 1; // UYVY 是单平面格式

        // 查询 buffer
        if (ioctl(dev_fd_, VIDIOC_QUERYBUF, &map_buff) < 0)
        {
            perror("querybuf");
            return false;
        }

        // 映射 plane 0
        v_mem_[i].mptr = (unsigned char *)mmap(
            NULL,
            map_buff.m.planes[0].length,
            PROT_READ | PROT_WRITE,
            MAP_SHARED,
            dev_fd_,
            map_buff.m.planes[0].m.mem_offset);

        v_mem_[i].size = map_buff.m.planes[0].length;

        if (v_mem_[i].mptr == MAP_FAILED)
        {
            perror("mmap");
            return false;
        }

        // 入队
        if (ioctl(dev_fd_, VIDIOC_QBUF, &map_buff) < 0)
        {
            perror("qbuf");
            return false;
        }
    }

    return true;
}

// 开始采集
bool OV_V4L2::start()
{
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    int ret = ioctl(dev_fd_, VIDIOC_STREAMON, &type);
    if (ret < 0)
    {
        perror("stream on error");
        return false;
    }

    // 从队列中提取一帧数据
    struct v4l2_buffer read_buff;
    struct v4l2_plane planes[1];
    read_buff.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    read_buff.memory = V4L2_MEMORY_MMAP;
    read_buff.m.planes = planes;
    read_buff.length = 1; // UYVY 是单平面格式

    //定义一个空间存储解码后的RGB数据
    unsigned char* rgbdata = new unsigned char[lcd_w_*lcd_h_*3];  //一帧数据所占空间
    memset(rgbdata, 0, sizeof(unsigned char) * lcd_w_*lcd_h_*3);
    while (true)// 循环读取
    {
        ret = ioctl(dev_fd_, VIDIOC_DQBUF, &read_buff);
        if (ret < 0)
        {
            perror("read buff error");
            return false;
        }

         //显示在LCD上
        Util::uyvy_to_rgb(v_mem_[read_buff.index].mptr, rgbdata, lcd_w_, lcd_h_);  //把yuyv数据解码为RGB数据（LCD只能显示RGB数据）
        //lcd_show_rgb(rgbdata, 640, 480);
        Util::lcd_to_argb(rgbdata, lcd_w_, lcd_h_, lcdptr_, lcd_w_, lcd_h_);

        // 读取完，将空间放回内核队列
        ret = ioctl(dev_fd_, VIDIOC_QBUF, &read_buff);
        if (ret < 0)
        {
            perror("put in queue erorr2");
            return false;
        }
    }
    delete[] rgbdata;

    return true;
}

bool OV_V4L2::stop()
{
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    int ret = ioctl(dev_fd_, VIDIOC_STREAMOFF, &type);
    if (ret < 0)
    {
        return false;
    }
    return true;
}

OV_V4L2::~OV_V4L2()
{
    // 释放映射空间
    if (v_mem_[0].mptr != nullptr)
    {
        for (int i = 0; i < 4; i++)
            munmap(v_mem_[i].mptr, v_mem_[i].size);
    }
    // 关闭文件表述符
    if (lcd_fd_ != -1)
        close(lcd_fd_);
    if (dev_fd_ != -1)
        close(dev_fd_);
}


void _rgb_convert_single(unsigned char Y, unsigned char U, unsigned char V,
        unsigned char& R, unsigned char& G, unsigned char& B)
{
    int r = Y + 1.4075*(V-128);
    int g = Y - 0.3455*(U-128) - 0.7169*(V-128);
    int b = Y + 1.7790*(U-128);

    // 限制范围到 [0,255]
    R = (r < 0) ? 0 : (r > 255 ? 255 : r);
    G = (g < 0) ? 0 : (g > 255 ? 255 : g);
    B = (b < 0) ? 0 : (b > 255 ? 255 : b);

}

void Util::uyvy_to_rgb(unsigned char *uyvydata, unsigned char *rgbdata, int w, int h)
{
    for (int i = 0; i < w * h / 2; i++)
    {
        unsigned char U  = uyvydata[i*4 + 0];
        unsigned char Y0 = uyvydata[i*4 + 1];
        unsigned char V  = uyvydata[i*4 + 2];
        unsigned char Y1 = uyvydata[i*4 + 3];

        _rgb_convert_single(Y0, U, V, rgbdata[i*6+0], rgbdata[i*6+1], rgbdata[i*6+2]);
        _rgb_convert_single(Y1, U, V, rgbdata[i*6+3], rgbdata[i*6+4], rgbdata[i*6+5]);
    }
}            

void Util::lcd_to_argb(unsigned char *rgbdata,
     int img_w, int img_h, unsigned int *lcd_ptr, int lcd_w, int lcd_h)
{
    for (int y = 0; y < img_h; y++)
    {
        for (int x = 0; x < img_w; x++)
            {
            // 计算原图中像素的位置
            int src_idx = ((img_h - 1 - y) * img_w + x) * 3;
            unsigned char r = rgbdata[src_idx + 0];
            unsigned char g = rgbdata[src_idx + 1];
            unsigned char b = rgbdata[src_idx + 2];

            // 将RGB转换为LCD的ARGB格式
            unsigned int color = 0xFF000000 | (r << 16) | (g << 8) | b;

            // 计算在LCD上的位置（居中显示）
            int lcd_x = x;
            int lcd_y = y;

            // 检查是否在屏幕范围内
            if (lcd_x < lcd_w && lcd_y < lcd_h)
            {
                lcd_ptr[lcd_y * lcd_w + lcd_x] = color;
            }
        }
    }
}
